#include "bt_manager/bt_manager.hpp"
#include "bt_manager/ros_interface.hpp"
#include <ros/package.h>
#include <ros/ros.h>
#include <thread>

int main(int argc, char const* argv[])
{
  ros::init(argc, argv, "bt_manager");
  auto blackboard = std::make_shared<Sentry_BT::Blackboard>();
  auto ros_interface_node = std::make_shared<Sentry_BT::ros_interface>(blackboard);
  auto transform_utils_node = std::make_shared<Sentry_BT::TransformUtils>();

  auto bt_blackboard = blackboard->getBTBlackboard();
  bt_blackboard->set("ros_interface", ros_interface_node);

  Sentry_BT::BTManager bt_manager;

  std::string pkg_path = ros::package::getPath("bt_manager");
  std::string xml_file_path = pkg_path + "/tree/nav_tree.xml";

  if(!bt_manager.initialize(xml_file_path, bt_blackboard))
  {
    ROS_ERROR("Failed to initialize BTManager with XML file: %s", xml_file_path.c_str());
    return -1;
  }

  ros::AsyncSpinner spinner(2);
  spinner.start();

  ros::Rate loop_rate(10);
  while(ros::ok())
  {
    bt_manager.execute();
    loop_rate.sleep();
  }

  spinner.stop();
  return 0;
}